Class FeatureManager
Defined in File feature_manager.h
Class Documentation
-
class FeatureManager
This class stores and calculates observed landmarks into map points. Triangulate feature points and get initial pose estimation after feature tracking FeatureTracker::trackImage().
Public Functions
-
explicit FeatureManager(Matrix3d _Rs[])
-
void setRic(Matrix3d _ric[])
Update transform between camera and IMU/INS.
- Parameters
_ric – Input extrinsic calibration matrix.
-
void clearState()
Clear all the map point memory when resetting.
-
int getFeatureCount()
Get valid map point number.
Note
Valid means used_num >=
4.- Returns
Map point feature size.
-
bool addFeatureCheckParallax(int frame_count, const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, double td)
Add the feature output from trackImage() to feature manager.
- Parameters
frame_count – The index of frame inside sliding window in slam estimator.
image – Not the real “image” but the std::map containing all the feature properties and camera ID as index.
td – Time interval between current and previous message time stamp.
- Returns
True if current frame satisfies the keyframe criteria.
-
vector<pair<Vector3d, Vector3d>> getCorresponding(int frame_count_l, int frame_count_r)
Get the vector of correspondences of frame pairs.
- Parameters
frame_count_l – Frame index 1.
frame_count_r – Frame index 2.
- Returns
Correspondence of points based on input frame indices.
-
void setDepth(const VectorXd &x)
Set the updated estimated depth into map point database.
- Parameters
x – The estimated inverse depth after optimization.
-
void removeFailures()
Erase the invalid map points (like estimated depth < 0)
-
void clearDepth()
Clear all the records of map point estimated depth with
-1.
-
VectorXd getDepthVector()
Acquire vector of depth for each valid map point (used_num >=
4).- Returns
Vector of inverse estimated depth.
-
void triangulate(int frameCnt, Vector3d Ps[], Matrix3d Rs[], Vector3d tic[], Matrix3d ric[])
Triangulates and calculates the estimated depth for each map point inside feature.
- Parameters
frameCnt – The index of frame inside sliding window in slam estimator.
Ps – Vector of estimated translation vector from all sliding window key frames to world coordinate.
Rs – Vector of estimated rotation matrices from all sliding window key frames to world coordinate.
tic – Extrinsic translation vector from camera to IMU/INS body frame.
ric – Extrinsic rotation matrix from camera to IMU/INS body frame.
-
void triangulatePoint(Eigen::Matrix<double, 3, 4> &Pose0, Eigen::Matrix<double, 3, 4> &Pose1, Eigen::Vector2d &point0, Eigen::Vector2d &point1, Eigen::Vector3d &point_3d)
- Parameters
Pose0 – Pose of a point at frame 1, from camera plane to world coordinate.
Pose1 – Pose of a point at frame 2, from camera plane to world coordinate.
point0 – 2D point coordinate of a point at frame 1, in camera plane.
point1 – 2D point coordinate of a point at frame 2, in camera plane.
point_3d – [out] Generated 3D point of this point in world coordinate.
-
void initFramePoseByPnP(int frameCnt, Vector3d Ps[], Matrix3d Rs[], Vector3d tic[], Matrix3d ric[])
Refine the vector of estimated Rs, Ps using EPnP.
- Parameters
frameCnt – The index of frame inside sliding window in slam estimator.
Ps – Vector of estimated translation vector from all sliding window key frames to world coordinate.
Rs – Vector of estimated rotation matrices from all sliding window key frames to world coordinate.
tic – Extrinsic translation vector from camera to IMU/INS body frame.
ric – Extrinsic rotation matrix from camera to IMU/INS body frame.
-
bool solvePoseByPnP(Eigen::Matrix3d &R_initial, Eigen::Vector3d &P_initial, vector<cv::Point2f> &pts2D, vector<cv::Point3f> &pts3D)
Refine pose estimation using cv::SolvePnP.
- Parameters
R_initial – Initial guess of rotation matrix of a frame.
P_initial – Initial guess of translation vector of a frame.
pts2D – Vector of 2D point coordinates in local camera coordinate of a frame.
pts3D – Corresponding 3D point coordinates in world coordinate.
- Returns
True if solve PnP is successful.
-
void removeBackShiftDepth(const Eigen::Matrix3d &marg_R, const Eigen::Vector3d marg_P, const Eigen::Matrix3d new_R, const Eigen::Vector3d new_P)
Remove oldest observation of a map point from feature and update initial estimated depth of each point.
Note
If a map point observation is empty, erase it and update the estimated depth using optimized pose of starting frame.
- Parameters
marg_R – The old initial rotation matrix in a sliding window waiting to be marginalized out.
marg_P – The old initial translation vector in a sliding window waiting to be marginalized out.
new_R – The new initial rotation matrix updated.
new_P – The new initial translation vector updated.
-
void removeBack()
Remove oldest observation of a map point from feature.
Note
If the map point observation is empty, erase it directly.
Public Members
-
list<FeaturePerId> feature
Database of all the map points with feature properties. List of FeaturePerId.
-
int last_track_num
Number of map points that have been observed before.
-
double last_average_parallax
-
int new_feature_num
Number of new observed map points.
-
int long_track_num
Number of map points that have been observed for more than /c times.
-
explicit FeatureManager(Matrix3d _Rs[])